//
// Created by pulsarv on 21-1-29.
//

#ifndef ROBOCAR_DATA_STRUCT_H
#define ROBOCAR_DATA_STRUCT_H

#include <string>
#include <mutex>
#include <map>
#include <atomic>
#include <opencv2/opencv.hpp>
#include <boost/thread/thread.hpp>

#define GYRO_SERIAL_BACK_SPEED_0D1HZ 0xffaa030100
#define GYRO_SERIAL_BACK_SPEED_0D5HZ 0xffaa030200
#define GYRO_SERIAL_BACK_SPEED_1HZ 0xffaa030300
#define GYRO_SERIAL_BACK_SPEED_2HZ 0xffaa030400
#define GYRO_SERIAL_BACK_SPEED_5HZ 0xffaa030500
#define GYRO_SERIAL_BACK_SPEED_10HZ 0xffaa030600
#define GYRO_SERIAL_BACK_SPEED_20HZ 0xffaa030700
#define GYRO_SERIAL_BACK_SPEED_50HZ 0xffaa030800
#define GYRO_SERIAL_BACK_SPEED_100HZ 0xffaa030900
#define GYRO_SERIAL_BACK_SPEED_ONCE 0xffaa030a00
#define GYRO_SERIAL_BACK_SPEED_200HZ 0xffaa030b00
#define GYRO_SERIAL_BACK_SPEED_CLOSE 0xffaa030c00


namespace rccore {
    namespace data_struct {
        typedef int COMMAND_TYPE;//命令类型
        typedef int ACTION_TYPE;//动作类型
        typedef int MOVER_PLUGIN;//移动驱动类型

        //命令类型
        enum {
            NONE_COMMAND = 0,//空命令
            MOVE_COMMAND,//移动命令
            SWITCH_COMMAND//开关命令 此命令传入bool类型值
        };
        //命令类型
        enum {
            MOVE_BY_MRAA = 0,//通过MRAA控制
            MOVE_BY_GPIO,//通过GPIO控制
            MOVE_BY_MOVE_PLUGIGS//通过底盘插件控制
        };

        //移动指令
        enum {
            MOVE_STOP = 0, //动作停止
            MOVE_AC, //逆时针旋转
            MOVE_CW, // 顺时针旋转
            MOVE_BACK, // 后退
            MOVE_FRONT, // 前进
            MOVE_LEFT, // 左移
            MOVE_RIGHT, // 右移
            MOVE_UP, // 爬升
            MOVE_DOWN, // 下降
            TAKE_OFF // 启动
        };

// TODO:状态机值
        enum {
            FOLLOW = 0, //跟随模式
            LAND, // 降落模式
            HOVER, // 覆盖模式（即发现了TAG）
            NORMAL, // 普通模式（啥都不做）
            LOSS_HOVER // 丢失TAG以后的状态
        };

//命令包
        class command_t {
        public:
            COMMAND_TYPE type = MOVE_COMMAND;//命令类型 默认移动控制
            ACTION_TYPE actionType = MOVE_STOP;//命令类型 默认停止
            float value = 0.0;//命令浮点类型值
            int int_value = 0;//命令整形值
            bool bool_value = false;//命令布尔类型值
            std::string string_value;//命令字符串类型值
        };

//TAG
        struct Apriltag {
            int id = 0;//TAG ID号
            int size = 150;//TAG 大小
        };

//配置信息
        struct ConfigInfo {
            // TODO:摄像头配置参数
            std::atomic<int> CAMERA_INDEX = 0; // 摄像头下标
            std::atomic<int> RESIZE_WIDTH = 640; // 摄像头下标
            std::atomic<int> RESIZE_HEIGHT = 480; // 摄像头下标
            std::atomic<float> CAMERA_DETCET_SIZE = 1.0f; // 摄像头下标
            std::atomic<bool> SAVE_VIDEO = false; // 视频保存开关
            std::atomic<int> CAMERA_FPS = 30; // 摄像头帧率设置
            std::string CAMERA_INFO = "CameraInfo.xml"; // 摄像头标定文件 默认值：CameraInfo.xml

            //TODO:激光雷达配置
            std::string LIDAR_DEVICE_PATH; //雷达串口地址

            //TODO:陀螺仪配置
            std::string GYRO_DEVICE_PATH; //陀螺仪串口地址
            std::atomic<int> GYRO_DEVICE_FREQ = 9600; //串口频率
            unsigned char GYRO_DEVICE_HEAD = 0x55; //数据包头
            std::atomic<int> GYRO_DEVICE_DATA_SIZE = 11; //数据包大小

            //TODO:底盘控制模块配置
            std::atomic<bool> USE_MRAA = true;//使用MRAA作为GPIO通信框架 否则为直接读写下位控制器
            std::atomic<int> GPIO_1;//制动端口号1
            std::atomic<int> PWM_1;//PWM端口号1
            std::atomic<int> GPIO_2;//制动端口号2
            std::atomic<int> PWM_2;//PWM端口号2
            std::atomic<int> GPIO_3;//制动端口号3
            std::atomic<int> PWM_3;//PWM端口号3
            std::string DSP_DEVICE_SERIAL_PATH;//下位控制器串口地址
            std::atomic<int> DSP_DEVICE_FREQ = 9600; //下位控制器串口频率


            // TODO:网络模块配置参数
            std::atomic<bool> HTTPD_ON = true;
            std::atomic<int> HTTPD_PORT = 20800;
            std::string REMOTE_SERVER_IPADDRESS = "127.0.0.1";
            std::atomic<int> REMOTE_SERVER_PORT = 100861;

            // TODO:卡尔曼滤波器配置参数
            //卡尔曼积分时间
            std::atomic<float> DELTA_T = 2.0f;
            //状态转移矩阵
            cv::Mat transitionMatrix = (cv::Mat_<float>(18, 18)
                    << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1
            );
            std::atomic<int> MAX_POINTS_SIZE = 100;
            std::atomic<bool> EKF_ON = false;

            std::atomic<bool> WHEEL_1_ON = true; // 1号轮开关
            std::atomic<float> WHEEL_1_MAX_V_M_S = 1.0f; // 1号轮最大速度
            std::atomic<float> WHEEL_1_P = 0.002f;// 1号轮PID参数P
            std::atomic<float> WHEEL_1_I = 0.002f;// 1号轮PID参数I
            std::atomic<float> WHEEL_1_D = 0.002f;// 1号轮PID参数D

            std::atomic<bool> WHEEL_2_ON = true; // 2号轮开关
            std::atomic<float> WHEEL_2_MAX_V_M_S = 1.0f; // 2号轮最大速度
            std::atomic<float> WHEEL_2_P = 0.002f;// 2号轮PID参数P
            std::atomic<float> WHEEL_2_I = 0.002f;// 2号轮PID参数I
            std::atomic<float> WHEEL_2_D = 0.002f;// 2号轮PID参数D

            std::atomic<bool> WHEEL_3_ON = true; // 3号轮开关
            std::atomic<float> WHEEL_3_MAX_V_M_S = 1.0f; // 3号轮最大速度
            std::atomic<float> WHEEL_3_P = 0.002f;// 2号轮PID参数P
            std::atomic<float> WHEEL_3_I = 0.002f;// 2号轮PID参数I
            std::atomic<float> WHEEL_3_D = 0.002f;// 2号轮PID参数D

            // TODO:系统配置参数
            std::atomic<bool> DEBUG_WINDOW = false; // 窗口开关
            std::atomic<bool> AUTO_CONTROL = false; // 自动控制开关
            std::atomic<int> LOSS_HOVER_TIMEOUT_TIME = 1000; // 丢失目标超时
            std::atomic<bool> USE_DETCET_PLUGUNS = true; // 使用插件
            std::string DETCET_PLUGUNS_PATH = "libhumandetect.so"; // 插件动态链接库路径

            // TODO:消息配置参数
            std::atomic<bool> HAVE_DSP_SERIAL_MESSAGE = true; // 下位控制器串口消息
            std::atomic<int> DSP_SERIAL_MESSAGE_QUEUE_SIZE = 2; // 下位控制器串口消息队列大小
            std::atomic<bool> HAVE_NETWORK_MESSAGE = true; // 网络服务消息
            std::atomic<int> NETWORK_MESSAGE_QUEUE_SIZE = 2; // 网络服务消息队列大小
            std::atomic<bool> HAVE_IMAGE_MESSAGE = true; // 图像消息
            std::atomic<int> IMAGE_MESSAGE_QUEUE_SIZE = 2; // 图像消息队列大小
            std::atomic<bool> HAVE_DEPTH_IMAGE_MESSAGE = true; // 深度图像消息
            std::atomic<int> DEPTH_IMAGE_MESSAGE_QUEUE_SIZE = 2; // 深度图像消息队列大小
            std::atomic<bool> HAVE_MOVE_MESSAGE = true; // 底盘控制消息
            std::atomic<int> MOVE_MESSAGE_QUEUE_SIZE = 2; // 底盘控制消息队列大小
            std::atomic<bool> HAVE_RADAR_MESSAGE = true; // 雷达消息
            std::atomic<int> RADAR_MESSAGE_QUEUE_SIZE = 2; // 雷达消息队列大小
            std::atomic<bool> HAVE_IMU_MESSAGE = true; // IMU消息
            std::atomic<int> IMU_MESSAGE_QUEUE_SIZE = 2; // IMU消息队列大小

            std::map<int, Apriltag> Apriltags;
        };
        struct STime {
            unsigned char ucYear;
            unsigned char ucMonth;
            unsigned char ucDay;
            unsigned char ucHour;
            unsigned char ucMinute;
            unsigned char ucSecond;
            unsigned short usMiliSecond;
        };
        struct SAcc {
            short a[3];
            short T;
        };
        struct SGyro {
            short w[3];
            short T;
        };
        struct SAngle {
            short Angle[3];
            short T;
        };
        struct SMag {
            short h[3];
            short T;
        };

        struct SDStatus {
            short sDStatus[4];
        };

        struct SPress {
            long lPressure;
            long lAltitude;
        };

        struct SLonLat {
            long lLon;
            long lLat;
        };

        struct SGPSV {
            short sGPSHeight;
            short sGPSYaw;
            long lGPSVelocity;
        };
        //系统信息
        struct SystemInfo {

            float IMU_ACC_X = 0.0;// X轴加速度
            float IMU_ACC_Y = 0.0;// Y轴加速度
            float IMU_ACC_Z = 0.0;// Z轴加速度

            float IMU_GYRO_X = 0.0;// X轴角速度
            float IMU_GYRO_Y = 0.0;// Y轴角速度
            float IMU_GYRO_Z = 0.0;// Z轴角速度

            float IMU_ANGLE_X = 0.0;// X轴角度
            float IMU_ANGLE_Y = 0.0;// Y轴角度
            float IMU_ANGLE_Z = 0.0;// Z轴角度

            float IMU_MAG_X = 0.0;// X轴磁场
            float IMU_MAG_Y = 0.0;// Y轴磁场
            float IMU_MAG_Z = 0.0;// Z轴磁场

            float WHEEL_1_V_M_S = 0.0;
            float WHEEL_1_A_V_M_S = 0.0;

            float WHEEL_2_V_M_S = 0.0;
            float WHEEL_2_A_V_M_S = 0.0;

            float WHEEL_3_V_M_S = 0.0;
            float WHEEL_3_A_V_M_S = 0.0;

            float EKF_ACTION_X_M_S = 0.0;
            float EKF_ACTION_Y_M_S = 0.0;
            float EKF_ACTION_Z_M_S = 0.0;

            float latitude_deg = 0.0;
            float longitude_deg = 0.0;

            float EKF_LAT = 0.0;
            float EKF_LNG = 0.0;

            float velocity_x_m_s = 0.0;
            float velocity_y_m_s = 0.0;
            float velocity_z_m_s = 0.0;

            float EKF_VX_M_S = 0.0;
            float EKF_VY_M_S = 0.0;
            float EKF_VZ_M_S = 0.0;

            float TRANS_X = 0.0;
            float TRANS_Y = 0.0;
            float TRANS_Z = 0.0;

            float EKF_TX = 0.0;
            float EKF_TY = 0.0;
            float EKF_TZ = 0.0;

            float EULAR_X = 0.0;
            float EULAR_Y = 0.0;
            float EULAR_Z = 0.0;


            float EKF_EX = 0.0;
            float EKF_EY = 0.0;
            float EKF_EZ = 0.0;

            float DISTANCE = 0.0;
            std::string ROBOT_MODE;
        };


        //车轮控制数据包
        typedef struct _rc_SerialPackage {
            char head;
            char sum_h;
            char sum_l;
        } rc_SerialPackage;

        class DOT {
        public:
            double alpha, theta, dist, x, y, z;

            //二维点云->极坐标转平面坐标
            DOT(double theta, double dist) :
                    theta(theta),
                    dist(dist),
                    x(cos(theta) * dist),
                    y(sin(theta) * dist),
                    z(0) {
            }

            //二维点云->极坐标转平面坐标
            DOT(double x, double y, int dim = 2) : alpha(0),
                                                   theta(atan(y / x)),
                                                   dist(sqrt(pow(x, 2) + pow(y, 2))),
                                                   x(x),
                                                   y(y),
                                                   z(0) {

            }

            //三维点云->球坐标转空间坐标
            DOT(double theta, double alpha, double dist) : alpha(alpha),
                                                           theta(theta),
                                                           dist(dist),
                                                           x(sin(theta) * cos(alpha) * dist),
                                                           y(sin(theta) * sin(alpha) * dist),
                                                           z(cos(theta) * dist) {
            }


            //三维点云->空间坐标转球坐标
            DOT(double x, double y, double z, int dim = 3) : alpha(atan(y / x)),
                                                             theta(atan(sqrt(pow(x, 2) + pow(y, 2)) / z)),
                                                             dist(sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))),
                                                             x(x),
                                                             y(y),
                                                             z(z) {
            }

        };

        struct JY901DATA {
            unsigned char data1;
            unsigned char data2;
            unsigned char data3;
            unsigned char data4;
            unsigned char data5;
            unsigned char data6;
            unsigned char data7;
            unsigned char data8;
            unsigned char data9;
            unsigned char data10;
            unsigned char sum;
        };


        class CJY901 {
        public:
            STime stcTime;
            SAcc stcAcc;
            SGyro stcGyro;
            SAngle stcAngle;
            SMag stcMag;
            SDStatus stcDStatus;
            SPress stcPress;
            SLonLat stcLonLat;
            SGPSV stcGPSV;

            CJY901();

            void CopeSerialData(char ucData[], unsigned short usLength);
        };
    }
}


#endif // ROBOCAR_DATA_STRUCT_H
